function xtrue = calculate_true_pose(xtrue, tr_param)
%
% INPUTS:

xtrue(1) = xtrue(1) + tr_param(1);
xtrue(2) = xtrue(2) + tr_param(2);
xtrue(3) = xtrue(3) + tr_param(3);
xtrue(4) = pi_to_pi(xtrue(4) + tr_param(4));
xtrue(5) = pi_to_pi(xtrue(5) + tr_param(5));
xtrue(6) = pi_to_pi(xtrue(6) + tr_param(6));
         
% xtrue= [xv(1) + V*dt*cos(G+xv(3,:)); 
%      xv(2) + V*dt*sin(G+xv(3,:));
%      pi_to_pi(xv(3) + V*dt*sin(G)/WB)];
 